﻿#include "gopt/gopt.h"
#include <Eigen/Eigen>
#include <iostream>
#include <vector>

class ConvexDistanceShape {
private:
    Eigen::MatrixXd A_;
    Eigen::VectorXd b_;
public:
    bool is_inside(const Eigen::VectorXd &x) {
        assert(A_.cols() == x.size());
        Eigen::MatrixXd dis = A_ * x - b_;
        return (dis.array() < 0).all();
    }
    double distance_sqrt(const Eigen::VectorXd &x, const Eigen::VectorXd &lambda) {
        return lambda.transpose() * (b_ - A_ * x);
    }
};

struct MyData {
  Eigen::MatrixXd A;
  Eigen::VectorXd b;
  Eigen::Vector2d x0;
};

double costFunction(void *instance, const Eigen::VectorXd &x,
                    Eigen::VectorXd &g) {
  auto data = static_cast<MyData *>(instance);
  auto gtmp = data->b - data->A * data->x0;
  double fx = x.transpose() * gtmp + (std::sqrt(5) + 10.0);
  g = 2.0 * fx * gtmp;
  return fx * fx;
}

double constraint_lambda(int index, void *instance, const Eigen::VectorXd &x,
                         Eigen::VectorXd &g) {
  g.fill(0);
  g[index] = -1;
  return -x[index];
}

void constraint_lambda0(void *instance, const Eigen::VectorXd &x,
                        Eigen::VectorXd &f, Eigen::MatrixXd &g) {
  f = -x;
  g.setIdentity();
  g = -g;
}

double constraint_norm(void *instance, const Eigen::VectorXd &x,
                       Eigen::VectorXd &g) {
  auto data = static_cast<MyData *>(instance);
  const auto B = data->A * data->A.transpose();
  g = 2.0 * B * x;
  auto fx1 = x.transpose() * B * x;
  auto fx2 = (x.transpose() * data->A).squaredNorm();
  return x.transpose() * B * x - 1.0;
}

int main(int argc, char **argv) {
  std::vector<Eigen::Vector2d> pts;
  pts.resize(3);
  pts[0] << 0.0, 0.0;
  pts[1] << 1.0, 1.0;
  pts[2] << 0.0, 1.0;
  Eigen::MatrixXd A;
  A.resize(pts.size(), 2);
  Eigen::VectorXd b;
  b.resize(pts.size());
  for (int j = pts.size() - 1, i = 0; i < pts.size(); j = i++) {
    const auto &p0 = pts[j];
    const auto &p1 = pts[i];
    const auto &dy = p1.y() - p0.y();
    const auto &dx = p1.x() - p0.x();
    A(i, 0) = dy;
    A(i, 1) = -dx;
    b(i) = dy * p0.x() - dx * p0.y();
  }
  std::cout << b << std::endl;
  Eigen::Vector2d t1;
  t1 << 0.5, 0.6;
  std::cout << A * t1 << std::endl;
  t1 << 2.0, 2.0;
  std::cout << A * t1 << std::endl;
  t1 << 1.0, 1.0;
  std::cout << A * t1 << std::endl;

  MyData data;
  data.A = A;
  data.b = b;
  data.x0 << 2.0, 3.0;

  double finalCost;
  Eigen::VectorXd x(3);
  x.fill(0);

  /* Set the initial guess */

  /* Set the minimization parameters */
  auto local_solver =
      std::shared_ptr<GOptSolver::BaseSolver>(new GOptSolver::LBFGSSolver());
  {
    auto tmp = std::static_pointer_cast<GOptSolver::LBFGSSolver>(local_solver);
    tmp->param.g_epsilon = 1.0e-3;
    tmp->param.delta = 1.0e-3;
  }

  GOptSolver::AugLagSolver solver;
  solver.set_local_solver(local_solver);
  solver.set_objective(costFunction, &data);

  //   for (int i = 0; i < 3; ++i) {
  //     auto cons = std::shared_ptr<GOptSolver::LagInEqConstraintNormal>(
  //         new GOptSolver::LagInEqConstraintNormal());
  //     cons->set_constraint(std::bind(constraint_lambda0, i,
  //     std::placeholders::_1,
  //                                    std::placeholders::_2,
  //                                    std::placeholders::_3,
  //                                    std::placeholders::_4),
  //                          1, 3, &data);
  //     solver.add_constraint(
  //         std::static_pointer_cast<GOptSolver::LagConstraintBase>(cons));
  //   }

  if (false) {
    auto cons = std::shared_ptr<GOptSolver::LagInEqConstraintNormal>(
        new GOptSolver::LagInEqConstraintNormal());
    cons->set_constraint(constraint_lambda0, 3, 3, &data);
    solver.add_constraint(
        std::static_pointer_cast<GOptSolver::LagConstraintBase>(cons));
  } 
  if (false) {
    auto cons = std::shared_ptr<GOptSolver::LagBoxContraint>(
        new GOptSolver::LagBoxContraint());
    Eigen::VectorXd lower_bounds, upper_bounds;
    lower_bounds.setZero(3);
    upper_bounds.setZero(3);
    upper_bounds.setConstant(HUGE_VAL);
    lower_bounds.setZero(3);
    cons->set_constraint(lower_bounds, upper_bounds);
    solver.add_constraint(
        std::static_pointer_cast<GOptSolver::LagConstraintBase>(cons));
  }

  if (true) {
    auto cons = std::shared_ptr<GOptSolver::LagBoundContraint>(
        new GOptSolver::LagBoundContraint());
    Eigen::VectorXd lower_bounds, upper_bounds;
    lower_bounds.setZero(3);
    cons->set_constraint(lower_bounds, false);
    solver.add_constraint(
        std::static_pointer_cast<GOptSolver::LagConstraintBase>(cons));
  }

  {
    auto cons = std::shared_ptr<GOptSolver::LagInEqConstraint>(
        new GOptSolver::LagInEqConstraint());
    cons->set_constraint(constraint_norm, &data);
    solver.add_constraint(
        std::static_pointer_cast<GOptSolver::LagConstraintBase>(cons));
  }

  auto ret = solver.minimize(x, finalCost);

  /* Report the result. */
  std::cout << std::setprecision(4)
            << "================================" << std::endl
            << "Ret: " << ret << " " << solver.get_local_ret() << std::endl
            << "Minimized Cost: " << finalCost << std::endl
            << "Optimal Variables: " << std::endl
            << x.transpose() << std::endl;

  return ret;
}
